#include <tool.hpp>




#ifdef use_ros && use_opencv
// ros 2 opencv 
void rosImg2opencvMat_color(sensor_msgs::ImageConstPtr img_msg, cv::Mat& outMat){
    cv_bridge::CvImageConstPtr ptr;
    if (img_msg->encoding == "8UC1")
    {
        sensor_msgs::Image img;
        img.header = img_msg->header;
        img.height = img_msg->height;
        img.width = img_msg->width;
        img.is_bigendian = img_msg->is_bigendian;
        img.step = img_msg->step;
        img.data = img_msg->data;
        img.encoding = "mono8";
        ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
    }
    else
        ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);
    (ptr->image).copyTo(outMat);// copy & return
}

// ros 2 opencv depth
void rosImg2opencvMat_depth(sensor_msgs::ImageConstPtr depth_msg, cv::Mat& outMat){
    cv_bridge::CvImageConstPtr depth_ptr;
    {
        sensor_msgs::Image img;
        img.header = depth_msg->header;
        img.height = depth_msg->height;
        img.width = depth_msg->width;
        img.is_bigendian = depth_msg->is_bigendian;
        img.step = depth_msg->step;
        img.data = depth_msg->data;
        img.encoding = sensor_msgs::image_encodings::MONO16;
        depth_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO16);
    }
    (depth_ptr->image).copyTo(outMat);
}

#endif